#!/usr/bin/env python
#-*- coding:UTF-8 -*-
import rospy
from uav.msg import Cmd

if __name__=="__main__":
	rospy.init_node('cmd_pub', anonymous=True)
	cmd_pub = rospy.Publisher('/uav/cmd', Cmd , queue_size=20)
	rate = rospy.Rate(20)


	while not rospy.is_shutdown():
		print("please input the number: ")
		num = input()
		cmds = []
		for i in range(num):
			print("input: [uavid,x,y,z,v/p]")
			cmd = Cmd()
			str=input()
			cmd.uavid = int(str[0])
			cmd.x = float(str[1])
			cmd.y = float(str[2])
			cmd.z = float(str[3])
			if str[4]=='v':
				cmd.type=1
			else:
				cmd.type=0

			cmds.append(cmd)
	
		for j in range(5):
			for i in range(num):
				cmd_pub.publish(cmds[i])
			rate.sleep()
        
	   
